﻿#pragma once

#include <xy/geom/vec.h>

namespace xy
{

namespace geom
{

enum class RotationOrder
{
    XYZ,
    XZY,
    YXZ,
    YZX,
    ZXY,
    ZYX
};

template <typename T> struct quat
{
    T w, x, y, z;

    /**
     * @brief 单位四元数
     *
     * @return quat
     */
    quat(T w = 1, T x = 0, T y = 0, T z = 0) : w(w), x(x), y(y), z(z)
    {
        // 自动归一化
        auto n = w * w + x * x + y * y + z * z;
        if (n > 0)
        {
            w /= n;
            x /= n;
            y /= n;
            z /= n;
        }
    }

    /**
     * @brief 通过实部和转轴初始化
     *
     * @param w
     * @param axis
     */
    quat(T w, const vec<3, T> &axis) : quat(w, axis.x(), axis.y(), axis.z())
    {
    }

    /**
     * @brief 从欧拉角构建四元数
     *
     * @param x
     * @param y
     * @param z
     * @param order
     * @return quat
     */
    static quat euler(T x, T y, T z, RotationOrder order = RotationOrder::ZXY)
    {
        x = math::degrees_to_radians(x);
        y = math::degrees_to_radians(y);
        z = math::degrees_to_radians(z);

        T cx = std::cos(x / 2);
        T sx = std::sin(x / 2);
        T cy = std::cos(y / 2);
        T sy = std::sin(y / 2);
        T cz = std::cos(z / 2);
        T sz = std::sin(z / 2);

        quat qx{cx, sx, 0, 0};
        quat qy{cy, 0, sy, 0};
        quat qz{cz, 0, 0, sz};

        switch (order)
        {
        case RotationOrder::XYZ:
            return qz * qy * qx;
        case RotationOrder::XZY:
            return qy * qz * qx;
        case RotationOrder::YXZ:
            return qz * qx * qy;
        case RotationOrder::YZX:
            return qx * qz * qy;
        case RotationOrder::ZXY:
            return qy * qx * qz;
        case RotationOrder::ZYX:
            return qx * qy * qz;
        default:
            return {};
        }
    }

    /**
     * @brief 转欧拉角
     *
     * @return vec<3, T>
     */
    vec<3, T> to_euler() const
    {
        auto sx2 = math::clamp(2 * (w * x - y * z), T{-1}, T{1});

        // 需要考虑 3 种情况
        auto eulerX = std::asin(sx2) / 2;
        auto sx = std::sin(eulerX);
        auto eulerY =
            std::abs(w * y + z * x) < math::numeric::distance ? 0 : std::atan2(x * x + y * y - sx * sx, w * y + z * x);
        auto eulerZ =
            std::abs(w * z + y * x) < math::numeric::distance ? 0 : std::atan2(x * x + z * z - sx * sx, w * z + y * x);
        return vec<3, T>{math::radians_to_degrees(eulerX), math::radians_to_degrees(eulerY),
                         math::radians_to_degrees(eulerZ)} *
               2;
    }

    /**
     * @brief 获得转动四元数
     *
     * @param from
     * @param to
     * @return quat
     */
    static quat from_to(const vec<3, T> &from_, const vec<3, T> &to_)
    {
        auto from = from_.normalized();
        auto to = to_.normalized();
        auto axis = cross(from, to);
        auto axisNorm = axis.modulus();

        if (axisNorm < math::numeric::distance)
        {
            if (dot(from, to) > 0)
                return {};
            else
                return {T{}, T{}, T{}, T{1}};
        }

        axis /= axisNorm;
        auto a = angle(from, to);
        auto w = std::cos(a / 2);
        auto xyz = axis * std::sin(a / 2);
        return {w, xyz.x(), xyz.y(), xyz.z()};
    }

    /**
     * @brief 获得绕 axis 轴旋转 angle 角度的四元数 q
     * p -> p' = q * p * q^-1
     *
     * @param[in] axis
     * @param[in] angle
     * @return quat
     */
    static quat rotate(const vec<3, T> &axis, T angle)
    {
        auto c = std::cos(angle / 2);
        auto s = std::sin(angle / 2);
        auto v = axis.normalized() * s;
        return {c, v.x(), v.y(), v.z()};
    }

    /**
     * @brief 转为矩阵
     *
     * @param[in] q
     * @return mat<4, 4, T>
     */
    static mat<4, 4, T> to_mat4x4(const quat &q)
    {
        auto m = mat<4, 4, T>::identity();
        T s = q.w, x = q.x, y = q.y, z = q.z;
        m(0, 0) = 2 * (s * s + x * x) - 1;
        m(0, 1) = 2 * (x * y - s * z);
        m(0, 2) = 2 * (x * z + s * y);
        m(1, 0) = 2 * (x * y + s * z);
        m(1, 1) = 2 * (s * s + y * y) - 1;
        m(1, 2) = 2 * (y * z - s * x);
        m(2, 0) = 2 * (x * z - s * y);
        m(2, 1) = 2 * (y * z + s * x);
        m(2, 2) = 2 * (s * s + z * z) - 1;
        return m;
    }

    /**
     * @brief 从矩阵构造四元数
     *
     * @param[in] m
     * @return quat
     */
    static quat from_mat4x4(const mat<4, 4, T> &m)
    {
        T trace = m(0, 0) + m(1, 1) + m(2, 2);
        if (trace > 0)
        {
            T s = std::sqrt(trace + 1) * 2;
            T inv = 1 / s;
            return {0.25 * s, (m(2, 1) - m(1, 2)) * inv, (m(0, 2) - m(2, 0)) * inv, (m(1, 0) - m(0, 1)) * inv};
        }
        else
        {
            // 如果 trace < 0，则 s = sqrt(1 + trace) 可能接近零，导致 0.5 / s 溢出，因此使用其它形式
            // 只需要找到对角线上最大的元素，作为正元素即可
            int i = m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2) ? 0 : (m(1, 1) > m(2, 2) ? 1 : 2);
            int j = (i + 1) % 3;
            int k = (i + 2) % 3;

            vec<3, T> axis;
            axis[i] = std::sqrt(1 + m(i, i) - m(j, j) - m(k, k)) * 2;
            T inv = 1 / axis[i];
            auto w = (m(k, j) - m(j, k)) * inv;
            axis[j] = (m(j, i) + m(i, j)) * inv;
            axis[k] = (m(k, i) + m(i, k)) * inv;
            axis[i] /= 4;
            return {w, axis.x(), axis.y(), axis.z()};
        }
    }

    /**
     * @brief 球面插值
     *
     * @param q1
     * @param q2
     * @param t
     * @return quat
     */
    static quat slerp(const quat &q1, const quat &q2, T t)
    {
        auto cosTheta = dot(q1, q2);
        if (cosTheta < 0)
        {
            q2 = -q2;
            cosTheta = -cosTheta;
        }

        auto theta = std::acos(math::clamp(cosTheta, T{-1}, T{1}));
        auto sinTheta = std::sin(theta);
        if (std::abs(sinTheta) < math::numeric::distance)
            return q1;
        else
        {
            auto s1 = std::sin((1 - t) * theta) / sinTheta;
            auto s2 = std::sin(t * theta) / sinTheta;
            return {q1.w * s1 + q2.w * s2, q1.x * s1 + q2.x * s2, q1.y * s1 + q2.y * s2, q1.z * s1 + q2.z * s2};
        }
    }

    /**
     * @brief 四元数乘法
     *
     * @param rhs
     * @return quat
     */
    quat operator*(const quat &rhs)
    {
        auto xyz = rhs.axis() * w + axis() * rhs.w + cross(axis(), rhs.axis());
        return {w * rhs.w - dot(axis(), rhs.axis()), xyz.x(), xyz.y(), xyz.z()};
    }

    /**
     * @brief 旋转
     *
     * @param rhs
     * @return vec<3, T>
     */
    vec<3, T> operator*(const vec<3, T> &rhs)
    {
        auto q = *this * quat{T{}, rhs} * conjugate();
        return {q.x, q.y, q.z};
    }

    /**
     * @brief 四元数乘法
     *
     * @param[in] q
     * @return quat&
     */
    quat &operator*=(const quat &q)
    {
        *this = *this * q;
        return *this;
    }

    /**
     * @brief 取反
     *
     * @return quat
     */
    quat operator-() const
    {
        return {-w, -x, -y, -z};
    }

    /**
     * @brief 是否相等
     *
     * @param[in] q
     * @return true
     * @return false
     */
    bool operator==(const quat &q) const
    {
        // 反向四元数相等
        return 1 - std::abs(dot(q, *this)) < math::numeric::distance;
    }

    /**
     * @brief 获得转轴
     *
     * @return vec<3, T>
     */
    vec<3, T> axis() const
    {
        return {x, y, z};
    }

    /**
     * @brief 共轭
     *
     * @return quat
     */
    quat conjugate() const
    {
        return {w, -x, -y, -z};
    }
};

using quatf = quat<float>;
using quatd = quat<double>;

/**
 * @brief 点积
 *
 * @tparam T
 * @param q1
 * @param q2
 * @return T
 */
template <typename T> inline T dot(const quat<T> &q1, const quat<T> &q2)
{
    return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
}

/**
 * @brief 输出四元数
 *
 * @param[in] os
 * @param[in] q
 * @return std::ostream&
 */
template <typename T> inline std::ostream &operator<<(std::ostream &os, const quat<T> &q)
{
    os << "{" << q.w << " + " << q.x << "i + " << q.y << "j + " << q.z << "k}";
    return os;
}

} // namespace geom

} // namespace xy